#include <opencv2/opencv.hpp>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>

#include <netdb.h>

#include <thread>
#include <mutex>
#include "armmove.hpp"

static int k=0;

static int pind[8]={0,0,0,0,0,0,0,0};

static void error(const char *msg)
{
    perror(msg);
    exit(1);
}
static int ServerStart(uint16_t portno)
{
//    socklen_t clilen;
    struct sockaddr_in serv_addr;


    int sockfd = socket(AF_INET, SOCK_STREAM, 0);
    if (sockfd < 0)
        error("ERROR opening socket");
    bzero((char *) &serv_addr, sizeof(serv_addr));

    serv_addr.sin_family = AF_INET;
    serv_addr.sin_addr.s_addr = INADDR_ANY;
    serv_addr.sin_port = htons(portno);

    const int       optVal = 1;
    const socklen_t optLen = sizeof(optVal);

    if(setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, (void*) &optVal, optLen)!=0)
    {
        error("ERROR on setsockopt");
    }

    if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0)
    {
        error("ERROR on binding");
    }

    listen(sockfd,5);
//    clilen = sizeof(cli_addr);

    return sockfd;
}


static ssize_t ServerSend(int sockfd, const void* pData, size_t nData)
{
    struct sockaddr_in cli_addr;
    socklen_t clilen;
    clilen = sizeof(cli_addr);

    int newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
    if (newsockfd < 0)
        error("ERROR on accept");

    std::cout<<"send frame\n"<<std::flush;

    ssize_t n = write(newsockfd,pData,nData);

    if (n < 0) {
        error("ERROR writing to socket");
    }

    std::cout<<"end send frame "<<n<<" \n"<<std::flush;


    close(newsockfd);
    return n;
}

static ssize_t ServerReceive(int sockfd, void* pData, size_t nData)
{
    struct sockaddr_in cli_addr;
    socklen_t clilen;
    clilen = sizeof(cli_addr);


    int newsockfd = accept(sockfd, (struct sockaddr *) &cli_addr, &clilen);
    if (newsockfd < 0)
        error("ERROR on accept");

    std::cout<<"receive pos\n"<<std::flush;

    ssize_t n = read(newsockfd,pData,nData);
    if (n < 0) {
        error("ERROR reading from socket");
    }

    close(newsockfd);

    std::cout<<"end receive pos "<< n <<" \n"<<std::flush;

    return n;
}


static void ClearThread(std::vector<std::thread> &ths, std::mutex &mtx, int &k)
{

}


static void RCVThread(int &k)
{
    int sockfd=ServerStart(8765);

    int n;
    char buffer[256];

    std::vector<std::thread> ths;
    std::mutex mtx;

    while(k!=27)
    {
        bzero(buffer,256);
        n=ServerReceive(sockfd, buffer, 255);
        int *pindex=(int*)buffer;

        if(pindex[0]>0 && pindex[1]>0 &&
                pindex[2]>0 && pindex[3]>0 &&
                pindex[4]>0 && pindex[5]>0 &&
                pindex[6]>0 && pindex[7]>0 )
        {

            memcpy(pind, pindex, n);



            cv::Vec2d *corner=new cv::Vec2d[4];
            corner[0][0]=pindex[0];
            corner[0][1]=pindex[1];
            corner[1][0]=pindex[2];
            corner[1][1]=pindex[3];
            corner[2][0]=pindex[4];
            corner[2][1]=pindex[5];
            corner[3][0]=pindex[6];
            corner[3][1]=pindex[7];

            if(ths.size()>10000)
            {
                std::cout<<"end first thread\n"<<std::flush;

                if(ths[0].joinable())
                    ths[0].join();
                ths.erase(ths.begin());
            }

            std::cout<<"add thread\n"<<std::flush;
                        ths.push_back(std::thread(ArmMove2, corner, std::ref(mtx)));
            //            ths.push_back(std::thread(ArmMove3, corner, std::ref(mtx)));
            //            ths.push_back(std::thread(ArmMove, pindex[0], pindex[1], std::ref(mtx)));
//            ths.push_back(std::thread(ArmMove4, std::ref(mtx)));

            std::cout<<"end add thread\n"<<std::flush;

        }
    }

     close(sockfd);

    for(auto &th:ths) th.join();
}



static void startserver(uint16_t portno, int index=0)
{

    cv::VideoCapture c(index);

    c.set(CV_CAP_PROP_FRAME_HEIGHT,480);
    c.set(CV_CAP_PROP_FRAME_WIDTH,640);

    int sockfd=ServerStart(portno);
    int n;

    cv::Mat m;

    std::thread th1(RCVThread, std::ref(k));

    while(k!=27)
    {
        std::cout<<"read frame\n"<<std::flush;

        if(!c.read(m))
            continue;


        std::cout<<"encode frame\n"<<std::flush;

        std::vector<unsigned char> mec;
        cv::imencode(".jpg",m,mec);

        std::cout<<"end encode frame\n"<<std::flush;
        n=ServerSend(sockfd, mec.data(),mec.size());

        std::cout<<"show img\n"<<std::flush;



        cv::line(m, cv::Point(pind[0],pind[1]), cv::Point(pind[2],pind[3]), cv::Scalar(0,0,255), 3 );

        cv::line(m, cv::Point(pind[2],pind[3]), cv::Point(pind[4],pind[5]), cv::Scalar(0,255,0), 3 );


        cv::line(m, cv::Point(pind[4],pind[5]), cv::Point(pind[6],pind[7]), cv::Scalar(255,0,0), 3 );


        cv::line(m, cv::Point(pind[6],pind[7]), cv::Point(pind[0],pind[1]), cv::Scalar(255,0,255), 3 );

        cv::imshow("m",m);
        k=cv::waitKey(1);

    }

    c.release();

    close(sockfd);

    th1.join();
}


int main(int argc, char *argv[])
{

    if(argc!=2)
        return 1;

    int ind=0;
    std::istringstream(argv[1])>>ind;
    startserver(2345, ind);

    return 0;
}

